#pragma once
#include "opencv2/opencv.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "slStruct.h"
#include <cuda_runtime.h>
#include "vector"
#include "map"
#include <opencv2/highgui/highgui.hpp>
// #include <Eigen/Core>
// #include <cusolverDn.h>
// #include <cublas_v2.h>
// #include <cusolver_common.h>
// #include <complex>

#ifndef slLog
#define slLog Log::getInstance()
#endif

#ifndef PI
#define PI 3.14159265359f
#endif

namespace SLCuda
{

    __device__ void getEpipolarLine(float *point, float *fundamentalMatrix, float *epipolarLine);

    __device__ void lineIntersect(float *line1, float *line2, float *result);

    __device__ void undistortPoint(float *p, float *instrinsic, float *kc, float *res);

    __device__ float length(float *p);

    __device__ float *normalize(float *p);

    __device__ float dot(float *p1, float *p2);

    __device__ float *cross(float *p1, float *p2);

    __device__ void cross(float *p1, float *p2, float *r);

    __device__ float getAngleCos(float *p1, float *p2);

    __device__ float getAngleCos(Vec3f p1, Vec3f p2);

    __device__ float getAngle(float *p1, float *p2);

    __device__ float getAngle(Vec3f p1, Vec3f p2);

    __device__ void multiply_44_41(float *mat, float *p, float *r);

    __device__ void multiply_33_31(float *mat, float *p, float *r);

    class Vec3f
    {
    public:
        float x, y, z;

        __device__ Vec3f(float _x = NAN, float _y = NAN, float _z = NAN) : x(_x), y(_y), z(_z) {}

        __device__ Vec3f operator+(const Vec3f &other) const;

        __device__ Vec3f operator-(const Vec3f &other) const;

        __device__ Vec3f operator*(const float s) const;

        __device__ Vec3f operator/(const float s) const;

        __device__ Vec3f normalize();

        __device__ void normalizeSelf();

        __device__ float length();

        __device__ float dot(const Vec3f &other);

        __device__ void cross(const Vec3f &other, Vec3f &r);

        __device__ Vec3f Vec3f::cross(const Vec3f &other);
    };

    __device__ void triangulatePoint(float *cameraPoint, float *projectorPoint, float *cameraIntrinsic, float *projectorIntrinsic, float *extrinsicMat, float *extrinsicRotation);

    __global__ void
    reconstructPhaseShiftMonocularCudaKernel(uchar *image, float *decodeImage, Size imageSize, float *fundamental, float *cameraIntrinsic, float *cameraKc, float *projectorIntrinsic, float *projectorKc, float *extrinsicMat, float *extrinsicRotation, float minDistance, float maxDistance, float *reconstutionPoint, uchar *reconstrutionColor);

    void reconstructPhaseShiftMonocularCuda(Mat &image, Mat &decodeImage, Size imageSize, Size patternSize, SL::ProjectorCalibrateTrinsics &projectorCalibrateTrinics, float minDistance, float maxDistance, SL::Points &points, SL::Colors &colors);

}